import matplotlib.pyplot as plt
import matplotlib
import csv
import h5py
import numpy as np

path = '/home/ubuntu/user_space/EKF-gilbert/EKFresult.csv'
pathRaw = '/home/ubuntu/user_space/Ronin/root_dir/a028_3/data.hdf5'

dt = 1.0 / 200.0

f = h5py.File(pathRaw, "a")
synced = f["synced"]
gyro = synced["gyro"]
acce = synced["acce"]

acce_x, acce_y, acce_z = [], [], []


for r in range(1, 20001) :
    acce_x.append(acce[r][0])
    acce_y.append(acce[r][1])
    acce_z.append(acce[r][2])

print(acce.shape)

#### plot quaternion part
pose = f["pose"]
tango_ori = pose["tango_ori"]
ekf_ori = pose["ekf_ori"]

tango_pos = pose["tango_pos"]

tango_q0, tango_q1, tango_q2, tango_q3 = [], [], [], []
ekf_q0, ekf_q1, ekf_q2, ekf_q3 = [], [], [], []
tango_px, tango_py, tango_pz = [], [], []

for r in range(1, 20001) :
    # tango_q0.append(tango_ori[r][0])
    # tango_q1.append(tango_ori[r][1])
    # tango_q2.append(tango_ori[r][2])
    # tango_q3.append(tango_ori[r][3])
    # ekf_q0.append(ekf_ori[r][0])
    # ekf_q1.append(ekf_ori[r][1])
    # ekf_q2.append(ekf_ori[r][2])
    # ekf_q3.append(ekf_ori[r][3])
    tango_px.append(tango_pos[r][0])
    tango_py.append(tango_pos[r][1])
    tango_pz.append(tango_pos[r][2])

print("Ori loaded")


fig, axes = plt.subplots(nrows = 3)
# time = np.arange(0, 158226, 1)
time = np.arange(0, 20000, 1)
#
axes[0].set(title = "x axis", ylabel = 'acce', xlabel = 'time')
axes[1].set(title = "y axis", ylabel = "acce", xlabel = "time")
axes[2].set(title = "z axis", ylabel = "acce", xlabel = "time")

axes[0].plot(time, acce_x, color = 'red')
axes[1].plot(time, acce_y, color = 'red')
axes[2].plot(time, acce_z, color = 'red')



r = csv.reader(open(path))
racce_x, racce_y, racce_z = [], [], []
#
r_q0, r_q1, r_q2, r_q3 = [], [], [], []
#
p_x, p_y, p_z = [], [], []
#
next(r)
cnt = 0
for row in r :
    if cnt == 20000:
        break
    print(row[7])
    racce_x.append(float(row[7]))
    racce_y.append(float(row[8]))
    racce_z.append(float(row[9]))
    r_q0.append(float(row[0]))
    r_q1.append(float(row[1]))
    r_q2.append(float(row[2]))
    r_q3.append(float(row[3]))
    p_x.append(float(row[4]))
    p_y.append(float(row[5]))
    p_z.append(float(row[6]))
    cnt += 1
#
#
print("Data loaded")
fig_r , axes_r = plt.subplots(nrows = 3)
axes_r[0].set(title = "x axis", ylabel = 'acce', xlabel = 'time')
axes_r[1].set(title = "y axis", ylabel = "acce", xlabel = "time")
axes_r[2].set(title = "z axis", ylabel = "acce", xlabel = "time")

axes[0].plot(time, racce_x, color = 'yellow')
axes[1].plot(time, racce_y, color = 'yellow')
axes[2].plot(time, racce_z, color = 'yellow')

# plt.show()


##### calculate tango acce and plot it, then compare it with My filter acce
# v0_x = (tango_px[1] - tango_px[0]) / dt
# v1_x = (tango_px[2] - tango_px[1]) / dt
# v0_y = (tango_py[1] - tango_py[0]) / dt
# v1_y = (tango_py[2] - tango_py[1]) / dt
# v0_z = (tango_px[1] - tango_px[0]) / dt
# v1_z = (tango_px[2] - tango_px[1]) / dt

v_tangop_x, v_tangop_y, v_tangop_z = [], [], []
a_tangop_x, a_tangop_y, a_tangop_z = [], [], []

for i in range(0, len(tango_px) - 2):
    v0_x = (tango_px[i + 1] - tango_px[i]) / dt
    v1_x = (tango_px[i + 2] - tango_px[i + 1]) / dt
    v0_y = (tango_py[i + 1] - tango_py[i]) / dt
    v1_y = (tango_py[i + 2] - tango_py[i + 1]) / dt
    v0_z = (tango_px[i + 1] - tango_px[i]) / dt
    v1_z = (tango_px[i + 2] - tango_px[i + 1]) / dt

    v_tangop_x.append(v0_x)
    v_tangop_y.append(v0_y)
    v_tangop_z.append(v0_z)

    a0_x = (v1_x - v0_x) / dt
    a0_y = (v1_y - v0_y) / dt
    a0_z = (v1_z - v0_z) / dt

    a_tangop_x.append(a0_x)
    a_tangop_y.append(a0_y)
    a_tangop_z.append(a0_z)


print(len(a_tangop_x))
a_tangop_x.append(0)
a_tangop_x.append(0)
a_tangop_y.append(0)
a_tangop_y.append(0)
a_tangop_z.append(0)
a_tangop_z.append(0)

fig_AA , axes_AA = plt.subplots(nrows = 3)
axes_AA[0].set(title = "x axis", ylabel = 'acce', xlabel = 'time')
axes_AA[1].set(title = "y axis", ylabel = "acce", xlabel = "time")
axes_AA[2].set(title = "z axis", ylabel = "acce", xlabel = "time")

# axes_AA[0].plot(time, acce_x, color = "red")
# axes_AA[1].plot(time, acce_y, color = "red")
# axes_AA[2].plot(time, acce_z, color = "red")
axes_AA[0].plot(time, racce_x, color = "red")
axes_AA[1].plot(time, racce_y, color = "red")
axes_AA[2].plot(time, racce_z, color = "red")


axes_AA[0].plot(time, a_tangop_x, color = "yellow")
axes_AA[1].plot(time, a_tangop_y, color = "yellow")
axes_AA[2].plot(time, a_tangop_z, color = "yellow")

plt.show()